package physics;

import graphics.Node;

import java.util.TreeSet;

public class PhysicsObject {
	//print impulse
	float inverseMass;
	Vector2f position;
	Vector2f velocity;
	Vector2f acceleration;
	Vector2f centerOfMass;
	float inverseMomentOfInertia;
	float orientation;
	float angularVelocity;
	Node renderable;
	
	public PhysicsObject() {
		inverseMass = 1;
		position = new Vector2f();
		velocity = new Vector2f();
		acceleration = new Vector2f();
		centerOfMass = new Vector2f();
		// inverseMomentOfInertia needs to be set in subclasses
	}

	public void updateState(float timePeriod) {
		position.sumScale(velocity, timePeriod);
		position.sumScale(acceleration, 
				Math.signum(timePeriod) * timePeriod * timePeriod / 2);
		velocity.sumScale(acceleration, timePeriod);
		orientation += angularVelocity * timePeriod;
		clearCaches();
	}
	
	public void updateRenderable() {
		if (renderable != null) {
			renderable.setTranslate(position.x, position.y, 0);
			renderable.setRotate((float)(180 * orientation / Math.PI), 0, 0, 1);
		}
	}

	public void clearCaches() {
	}

	public CollisionInfo getCollision(PhysicsObject other, final float time) {
		if (inverseMass == 0 && other.inverseMass == 0)
			return null;
		if (this instanceof HalfSpace && other instanceof Circle)
			return getCollision((HalfSpace)this, (Circle)other);
		if (this instanceof Circle && other instanceof HalfSpace) {
			CollisionInfo cInfo = getCollision((HalfSpace)other, (Circle)this);
			if (cInfo != null)
				cInfo.normal.scale(-1);
			return cInfo;
		}
		if (this instanceof HalfSpace && other instanceof Triangle)
			return getCollision((HalfSpace)this, (Triangle)other);
		if (this instanceof Triangle && other instanceof HalfSpace) {
			CollisionInfo cInfo = getCollision((HalfSpace)other, (Triangle)this);
			if (cInfo != null)
				cInfo.normal.scale(-1);
			return cInfo;
		}
		if (this instanceof Circle && other instanceof Circle)
			return getCollision((Circle)this, (Circle)other);
		if (this instanceof Circle && other instanceof Triangle)
			return getCollision((Circle)this, (Triangle)other);
		if (this instanceof Triangle && other instanceof Circle) {
			CollisionInfo cInfo = getCollision((Circle)other, (Triangle)this);
			if (cInfo != null)
				cInfo.normal.scale(-1);
			return cInfo;
		}
		if (this instanceof Triangle && other instanceof Triangle)
		{	
			//return getCollision((Triangle)this, (Triangle)other);
			CollisionInfo cInfo = getCollision((Triangle)other, (Triangle)this, time);
			if (cInfo != null)
			{
				cInfo.normal.scale(-1);
				//print ("tri-tri collision"+this.velocity.x);
			}
			return cInfo;
		}
		return null;
	}
	
	private  CollisionInfo getCollision(HalfSpace a, Circle b) {
		float distance = a.normal.dot(b.position);
		distance -= a.intercept;
		if (distance >= b.radius)
			return null;
		CollisionInfo cInfo = new CollisionInfo();
		cInfo.depth = b.radius - distance;
		cInfo.normal = new Vector2f(a.normal);
		cInfo.positionA = new Vector2f(b.position);
		cInfo.positionA.sumScale(cInfo.normal, -b.radius + cInfo.depth);
		cInfo.positionB = new Vector2f(b.position);
		cInfo.positionB.sumScale(cInfo.normal, -b.radius);
		return cInfo;
	}
	
	private  CollisionInfo getCollision(Circle a, Circle b) {
		Vector2f tmp = new Vector2f(b.position);
		tmp.sumScale(a.position, -1);
		float distance = tmp.length() - a.radius - b.radius;
		if (distance >= 0)
			return null;
		CollisionInfo cInfo = new CollisionInfo();
		cInfo.depth = -distance;
		tmp.normalize();
		cInfo.normal = tmp;
		cInfo.positionA = new Vector2f(a.position);
		cInfo.positionA.sumScale(cInfo.normal, a.radius);
		cInfo.positionB = new Vector2f(b.position);
		cInfo.positionB.sumScale(cInfo.normal, -b.radius);
		return cInfo;
	}
	
	private  CollisionInfo getCollision(HalfSpace a, Triangle b) {
		/*
		 *Process:
		 * get Vertices
		 * create table: 
		 *   f(vertices, normal, intercept) = distances
		 * find minimum of that table, and mark that index
		 * 
		 * the minIndex also happens to be the same index for the cooresponding vertex
		 */
		Vector2f[] vertices = b.getVertices();
				float[] distances = new float[vertices.length];//initialized
		//calculate distances
		for (int i = 0; i < vertices.length; i++) {
			distances[i] = a.normal.dot(vertices[i]) - a.intercept;
		}
			
		int minIndex = 0;//initialized
		//calculate minIndex (closest)
		for (int i = 1; i < distances.length; i++)
			if (distances[i] < distances[minIndex])
				minIndex = i;
		//if not close, no collision
		if (distances[minIndex] >= 0)
			return null;
		CollisionInfo cInfo = new CollisionInfo();
		cInfo.depth = -distances[minIndex];
		cInfo.normal = new Vector2f(a.normal);
		cInfo.positionA = new Vector2f(vertices[minIndex]);
		cInfo.positionA.sumScale(cInfo.normal, cInfo.depth);
		cInfo.positionB = new Vector2f(vertices[minIndex]);
		return cInfo;
	}
	
	private  CollisionInfo getCollision(Circle circle, Triangle tri) {
		Vector2f[] triangle_vertices = tri.getVertices();
		Vector2f[] tri_normals = tri.getNormals();
		float[] distance_test = new float[triangle_vertices.length];
		
		/*Gets the distance away using normal dot product and radius of circle*/
		for (int i = 0; i < triangle_vertices.length; i++) {
			Vector2f tmp = new Vector2f(circle.position); //why does this have to be here and not outside?
			tmp.sumScale(triangle_vertices[i], -1);
			distance_test[i] = tmp.dot(tri_normals[i]) - circle.radius;
		}
		
		int maxIndex = 0;
		
		/*find the maxIndex*/
		for (int i = 1; i < distance_test.length; i++)
		{
			if (distance_test[i] > distance_test[maxIndex])
				maxIndex = i;
		}
		if (distance_test[maxIndex] >= 0)
			return null;
		
		/*the following matrices are supposed to be sychronized:
		 * dist_test
		 * tri_normals
		 * */
		CollisionInfo cInfo = new CollisionInfo();
		cInfo.depth = -distance_test[maxIndex];
		cInfo.normal = new Vector2f(tri_normals[maxIndex]);
		cInfo.normal.scale(-1);
		cInfo.positionA = new Vector2f(circle.position);
		cInfo.positionA.sumScale(cInfo.normal, circle.radius);
		cInfo.positionB = new Vector2f(circle.position);
		cInfo.positionB.sumScale(cInfo.normal, circle.radius - cInfo.depth);
		return cInfo;
	}
	
	//This is the one to modify
	private  CollisionInfo getCollision(final Triangle a, final Triangle b, 
			final float time) {
		TreeSet<CollisionInfo> collisions = getCollisions(a, b);
		if (collisions.size() == 0) collisions = getLineIntersectionMain(a, b, time);
		if (collisions == null || collisions.size() == 0) return null;
		final CollisionInfo last = collisions.pollLast();
		if (collisions.size() == 0) {
			return last;
		} else if (last.depth == collisions.last().depth) {
			return averageCol(last, collisions.last());
		}
		return last;
	}
	
	private TreeSet<CollisionInfo> getCollisions(Triangle a, Triangle b) {
		final Vector2f[] a_verts = a.getVertices();
		final TreeSet<CollisionInfo> collisions = new TreeSet<CollisionInfo>();
		CollisionInfo temp;
		for (int i = 0; i < a_verts.length; i++) {
			temp = getPointInsideTriangle(b, a_verts[i]);
			if (temp != null) collisions.add(temp);
		}
		if(collisions.size() == 0) {
			final Vector2f[] b_verts = b.getVertices();
			for (int j = 0; j < b_verts.length; j++) {
				temp = getPointInsideTriangle(a, b_verts[j]);
				if (temp != null) {
					temp.normal.scale(-1);
					collisions.add(temp);
				}
			}
		}
		return collisions;
	}

	private TreeSet<CollisionInfo> getLineIntersectionMain(Triangle a, Triangle b, 
			final float time) {
		if (!getLineIntersection(a, b)) return null;
		TreeSet<CollisionInfo> temp = null;
		float step_size = time / 2;
		int direction = -1;
		int old_direction = 1;
		float total_time = time;
		while (temp == null) {
			total_time += direction * step_size;
			a.updateState(direction * step_size);
			b.updateState(direction * step_size);
			temp = getCollisions(a, b);
			if (temp.size() == 0) {
				temp = null;
				old_direction = direction;
				if (getLineIntersection(a, b)) {
					direction = -1;
				} else {
					direction = 1;
				}
				if (direction != old_direction) step_size /= 2;
			} else {
				for (CollisionInfo col : temp) col.time = time - total_time;
			}
		}
		return temp;
	}

	private boolean getLineIntersection(Triangle a, Triangle b) {
		final Vector2f[] a_verts = a.getVertices();
		final int[] a_vert_collide = new int[a_verts.length];
		final Vector2f[] b_verts = b.getVertices();
		for (int i = 0; i < a_verts.length; i++) {
			for (int j = 0; j < b_verts.length; j++) {
				if (edgeOverlap(a_verts[i], a_verts[(i+1)%a_verts.length], 
						b_verts[j], b_verts[(j+1)%b_verts.length])) {
					a_vert_collide[i]++;
					a_vert_collide[(i+1)%a_vert_collide.length]++;
				}
			}
		}
		for (int k = 0; k < a_verts.length; k++) {
			if (a_vert_collide[k] == 4) return true;
		}
		return false;
	}
	
	public static boolean edgeOverlap(final Vector2f P1, final Vector2f P2, 
			final Vector2f P3, final Vector2f P4) {
		final float divisor = 
				(P4.y - P3.y) * (P2.x - P1.x) - (P4.x - P3.x) * (P2.y - P1.y);
		if (divisor == 0) return false;
		final float ua = ((P4.x-P3.x)*(P1.y-P3.y)-(P4.y-P3.y)*(P1.x-P3.x));
		final float ub = ((P2.x-P1.x)*(P1.y-P3.y)-(P2.y-P1.y)*(P1.x-P3.x));
		if (divisor < 0) return divisor < ua && ua < 0 && divisor < ub && ub < 0;
		return 0 < ua && ua < divisor && 0 < ub && ub < divisor;
	}

	private CollisionInfo averageCol(final CollisionInfo last, 
			final CollisionInfo last2) {
		final CollisionInfo temp = new CollisionInfo();
		temp.depth = last.depth;
		temp.normal = last.normal;
		temp.positionA.x = (last.positionA.x + last2.positionA.x) /2;
		temp.positionA.y = (last.positionA.y + last2.positionA.y) /2;
		temp.positionB.x = (last.positionB.x + last2.positionB.x) /2;
		temp.positionB.y = (last.positionB.y + last2.positionB.y) /2;
		return temp;
	}

	public void resolveCollision(PhysicsObject other, CollisionInfo cInfo) {
		// Calculate the velocity of the collision point on the calling object.
		Vector2f relativeCollisionPositionA = new Vector2f(cInfo.positionA);
		relativeCollisionPositionA.sumScale(position, -1);
		relativeCollisionPositionA.sumScale(centerOfMass, -1);
		Vector2f linearVelocityA = new Vector2f(-relativeCollisionPositionA.y, relativeCollisionPositionA.x);
		linearVelocityA.scale(angularVelocity);
		linearVelocityA.sum(velocity);
		// Calculate the velocity of the collision point on the other object.
		Vector2f relativeCollisionPositionB = new Vector2f(cInfo.positionB);
		relativeCollisionPositionB.sumScale(other.position, -1);
		relativeCollisionPositionB.sumScale(other.centerOfMass, -1);
		Vector2f linearVelocityB = new Vector2f(-relativeCollisionPositionB.y, relativeCollisionPositionB.x);
		linearVelocityB.scale(other.angularVelocity);
		linearVelocityB.sum(other.velocity);
		// Calculate the relative velocity between the calling object and
		// other object, as if the calling object were stationary and only
		// the other object were moving.
		Vector2f relativeVelocity = new Vector2f(linearVelocityB);
		relativeVelocity.sumScale(linearVelocityA, -1);
		// Calculate the component of the relative velocity that lays along
		// the collision normal.
		float compRelVelAlongNormal = relativeVelocity.dot(cInfo.normal);
		// Calculate the resulting impulse per unit mass.
		//2.001 = fun
		//1.7 = loss
		//2 = energy conserved
		
		float impulse = (float)(1.9 * compRelVelAlongNormal / (
				inverseMass + other.inverseMass + 
				Math.pow(relativeCollisionPositionA.cross(cInfo.normal), 2) * inverseMomentOfInertia +
				Math.pow(relativeCollisionPositionB.cross(cInfo.normal), 2) * other.inverseMomentOfInertia));
		// Adjust the linear and angular velocities of each object in proportion
		// to their effective masses.
		velocity.sumScale(cInfo.normal, impulse * inverseMass);
		other.velocity.sumScale(cInfo.normal, -impulse * other.inverseMass);
		angularVelocity += relativeCollisionPositionA.cross(cInfo.normal) * impulse * inverseMomentOfInertia;
		other.angularVelocity -= relativeCollisionPositionB.cross(cInfo.normal) * impulse * other.inverseMomentOfInertia;

		// Calculate the amount of object overlap per unit mass.
		float depth = (float)(cInfo.depth / (
				inverseMass + other.inverseMass + 
				Math.pow(relativeCollisionPositionA.cross(cInfo.normal), 2) * inverseMomentOfInertia +
				Math.pow(relativeCollisionPositionB.cross(cInfo.normal), 2) * other.inverseMomentOfInertia));
		// Adjust the position and orientation  of each object in proportion
		// to their effective masses to remove overlap.
		
		position.sumScale(cInfo.normal, -depth * inverseMass);
		other.position.sumScale(cInfo.normal, depth * other.inverseMass);
		orientation -= relativeCollisionPositionA.cross(cInfo.normal) * depth * inverseMomentOfInertia;
		other.orientation += relativeCollisionPositionB.cross(cInfo.normal) * depth * other.inverseMomentOfInertia;
		
		clearCaches();
		other.clearCaches();
		if (cInfo.time != 0) {
			other.updateState(cInfo.time);
			updateState(cInfo.time);
		}
	}
	
	private CollisionInfo getPointInsideTriangle(final Triangle tri, 
			final Vector2f point) {
		Vector2f[] triangle_vertices = tri.getVertices();
		Vector2f[] tri_normals = tri.getNormals();
		float[] distance_test = new float[triangle_vertices.length];
		int maxIndex = 0;
		for (int i = 0; i < triangle_vertices.length; i++) {
			Vector2f tmp = new Vector2f(point);
			tmp.sumScale(triangle_vertices[i], -1);
			distance_test[i] = tmp.dot(tri_normals[i]);
			if (distance_test[i] > distance_test[maxIndex]) maxIndex = i;
		}
		if (distance_test[maxIndex] > 0)
			return null;
		CollisionInfo cInfo = new CollisionInfo();
		cInfo.depth = -distance_test[maxIndex];
		cInfo.normal = new Vector2f(tri_normals[maxIndex]);
		cInfo.normal.scale(-1);
		cInfo.positionA = new Vector2f(point);
		cInfo.positionB = new Vector2f(point);
		cInfo.positionB.sumScale(cInfo.normal, -cInfo.depth);
		return cInfo;
	}
}
